﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Mahanet.Abyss.PathCalculation
{
	public class Line3D
	{
		#region C'tor

		public Line3D(Coordinate3D linePoint, Vector3D direction)
		{
			if (direction.IsZero())
			{
				throw new ArgumentException("direction");
			}

			this.LinePoint = linePoint;
			this.NormalizedDirection = direction.Normalized();
		}

		public Line3D(Coordinate3D firstPoint, Coordinate3D secondPoint)
		{
			if (firstPoint.Equals(secondPoint))
			{
				throw new ArgumentException("direction");
			}

			this.LinePoint = firstPoint;
			this.NormalizedDirection = new Vector3D(firstPoint, secondPoint).Normalized();
		}

		#endregion

		#region Public Properties

		public Coordinate3D LinePoint { get; private set; }
		public Vector3D NormalizedDirection { get; private set; }

		#endregion

		#region Public Methods

		public Coordinate3D GetMinimalEquidistantPoint(Line3D other)
		{
			Coordinate3D pointOnThisLine, pointOnOtherLine;
			GetClosestPoints(other, out pointOnThisLine, out pointOnOtherLine);

			if (pointOnThisLine == null || pointOnOtherLine == null)
			{
				return null;
			}

			return pointOnThisLine.MidpointTo(pointOnOtherLine);
		}

		public void GetClosestPoints(Line3D other, out Coordinate3D pointOnThisLine, out Coordinate3D pointOnOtherLine)
		{
			double linesDot = this.NormalizedDirection.Dot(other.NormalizedDirection);
			if (PrecisionUtils.AreEqual(linesDot, 1))
			{
				// The lines are parallel; there are infinite points with the distance between them
				pointOnOtherLine = pointOnThisLine = null;
				return;
			}

			Vector3D crossingVector = other.LinePoint - this.LinePoint;
			double t0 =
				(linesDot * crossingVector.Dot(other.NormalizedDirection) - crossingVector.Dot(this.NormalizedDirection))
				/
				(linesDot * linesDot - 1);

			double t1 = linesDot * t0 - crossingVector.Dot(other.NormalizedDirection);

			pointOnThisLine = this.LinePoint + t0 * this.NormalizedDirection;
			pointOnOtherLine = other.LinePoint + t1 * other.NormalizedDirection;
		}

		#endregion
	}
}
